/***************************************************************************\
 * Copyright (C) by Keio University
 * Tridelity-3D-Screen.cpp created in 11 2011.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * Tridelity-3D-Screen.cpp is part of the HVRL Library.
 *
 * The HVRL Library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include <hvrl/display/Tridelity-3D-Screen.hpp>
#include <hvrl/common/Common.hpp>

namespace hvrl {
namespace tridelity {

/*const unsigned int output_width  = 1920;
 const unsigned int output_height = 1200;
 const unsigned int input_width  = 1920/5;
 const unsigned int input_height = 1200/5;*/

const unsigned int number_input = 5;

/*
 * According to Tridelity specs, pixels are defined as follow:
 * R5G4B3 R2G1B5 R4G3B2 R1G5B4 R3G2B1 ......
 * R1G5B4 R3G2B1 R5G4B3 R2G1B5 R4G3B2 ......
 * R2G1B5 R4G3B2 R1G5B4 R3G2B1 R5G4B3 ......
 * R3G2B1 R5G4B3 R2G1B5 R4G3B2 R1G5B4 ......
 * R4G3B2 R1G5B4 R3G2B1 R5G4B3 R2G1B5 ......
 * ...... ...... ...... ...... ...... ......
 * Where the tuple RGB represents a single pixel, and [1-5] are the ID of the input images.
 */

bool createImage(cv::Mat (&input)[5], cv::Mat& output) {

	/*// Check the size of input images
	 for(unsigned int i = 0; i < number_input; ++i)
	 {
	 if(input[i].rows != input_height || input[i].cols != input_width)
	 {
	 Log::add().error("tridility::createImage","Size of input images is not correct");
	 return false;
	 }
	 }

	 // Check the size of the output image
	 if(output.rows != 1200 || output.cols != 1920 || output.type() != input[0].type())
	 {
	 output = cv::Mat(output_height, output_width, input[0].type());
	 }*/
	unsigned int output_width = input[0].size().width*2.5;
	unsigned int output_height = input[0].size().height*2;
	output = cv::Mat(output_height, output_width, input[0].type());

	for (unsigned int i = 0; i < output_height; ++i) {

		int v= i%5;

		for (unsigned int j = 0; j < output_width; ++j) {

			int h = j%5;

			int coordxinput = j/(2.5);
			int coordyinput = i/(2);

			if((v==0 && h==0) || (v==1 && h==2) || (v==3 && h==1) || (v==2 && h==4) || (v==4 && h==3)){
				output.data[(i*output_width+j)*3] = input[4].data[(coordyinput*input[0].size().width+coordxinput)*3];
				output.data[(i*output_width+j)*3+1] = input[3].data[(coordyinput*input[0].size().width+coordxinput)*3+1];
				output.data[(i*output_width+j)*3+2] = input[2].data[(coordyinput*input[0].size().width+coordxinput)*3+2];
			}
			if((v==0 && h==1) || (v==1 && h==3) || (v==2 && h==0) || (v==3 && h==2) || (v==4 && h==4)){
				output.data[(i*output_width+j)*3] = input[1].data[(coordyinput*input[0].size().width+coordxinput)*3];
				output.data[(i*output_width+j)*3+1] = input[0].data[(coordyinput*input[0].size().width+coordxinput)*3+1];
				output.data[(i*output_width+j)*3+2] = input[4].data[(coordyinput*input[0].size().width+coordxinput)*3+2];

			}
			if((v==0 && h==2) || (v==1 && h==4) || (v==2 && h==1) || (v==3 && h==3) || (v==4 && h==0)){
				output.data[(i*output_width+j)*3] = input[3].data[(coordyinput*input[0].size().width+coordxinput)*3];
				output.data[(i*output_width+j)*3+1] = input[2].data[(coordyinput*input[0].size().width+coordxinput)*3+1];
				output.data[(i*output_width+j)*3+2] = input[1].data[(coordyinput*input[0].size().width+coordxinput)*3+2];

			}
			if((v==1 && h==0) || (v==0 && h==3) || (v==2 && h==2) || (v==3 && h==4) || (v==4 && h==1)){
				output.data[(i*output_width+j)*3] = input[0].data[(coordyinput*input[0].size().width+coordxinput)*3];
				output.data[(i*output_width+j)*3+1] = input[4].data[(coordyinput*input[0].size().width+coordxinput)*3+1];
				output.data[(i*output_width+j)*3+2] = input[3].data[(coordyinput*input[0].size().width+coordxinput)*3+2];

			}
			if((v==1 && h==1) || (v==0 && h==4) || (v==3 && h==0) || (v==2 && h==3) || (v==4 && h==2)){
				output.data[(i*output_width+j)*3] = input[2].data[(coordyinput*input[0].size().width+coordxinput)*3];
				output.data[(i*output_width+j)*3+1] = input[1].data[(coordyinput*input[0].size().width+coordxinput)*3+1];
				output.data[(i*output_width+j)*3+2] = input[0].data[(coordyinput*input[0].size().width+coordxinput)*3+2];

			}

		}
	}


/*
	// Process the input images
	for (unsigned int i = 0; i < output_height; ++i) {
		float yy = fmod(float(i),5.0f);
		int xx2 = i ;
		for (unsigned int j = 0; j < output_width; ++j) {

			float xx = fmod(float(j),5.0f);
			int yy2 = j ;
			float pre1 = std::floor(xx * 0.5f);
			float pre2 = 3.0f * fmod(xx, 2.0f);
			int rest = int( fmod(float(pre1 + pre2 + yy),5.0f));
			if (rest == 3) {
				 output.data[(i*output_width+j)*3] = input[0].data[(xx2*input[0].size().width+yy2)*3];
				 output.data[(i*output_width+j)*3+1] = input[1].data[(xx2*input[0].size().width+yy2)*3+1];
				 output.data[(i*output_width+j)*3+2] = input[2].data[(xx2*input[0].size().width+yy2)*3+2];
			}
			if (rest == 0) {
				 output.data[(i*output_width+j)*3] = input[2].data[(xx2*input[0].size().width+yy2)*3];
				 output.data[(i*output_width+j)*3+1] = input[3].data[(xx2*input[0].size().width+yy2)*3+1];
				 output.data[(i*output_width+j)*3+2] = input[4].data[(xx2*input[0].size().width+yy2)*3+2];
			}
			if (rest == 2) {
				 output.data[(i*output_width+j)*3] = input[4].data[(xx2*input[0].size().width+yy2)*3];
				 output.data[(i*output_width+j)*3+1] = input[0].data[(xx2*input[0].size().width+yy2)*3+1];
				 output.data[(i*output_width+j)*3+2] = input[1].data[(xx2*input[0].size().width+yy2)*3+2];
			}
			if (rest == 4) {
				 output.data[(i*output_width+j)*3] = input[1].data[(xx2*input[0].size().width+yy2)*3];
				 output.data[(i*output_width+j)*3+1] = input[2].data[(xx2*input[0].size().width+yy2)*3+1];
				 output.data[(i*output_width+j)*3+2] = input[3].data[(xx2*input[0].size().width+yy2)*3+2];
			}
			if (rest == 1) {
				 output.data[(i*output_width+j)*3] = input[3].data[(xx2*input[0].size().width+yy2)*3];
				 output.data[(i*output_width+j)*3+1] = input[4].data[(xx2*input[0].size().width+yy2)*3+1];
				 output.data[(i*output_width+j)*3+2] = input[0].data[(xx2*input[0].size().width+yy2)*3+2];
			}
		}
	}
*/
	/*  float tx = gl_TexCoord[0].x * HALF;
	 float ty = gl_TexCoord[0].y * ONE_THIRD;
	 vec2 coord0 = vec2(tx, ty + TWO_THIRD);
	 vec2 coord1 = vec2(tx + HALF, ty + ONE_THIRD);
	 vec2 coord2 = vec2(tx, ty + ONE_THIRD);
	 vec2 coord3 = vec2(tx + HALF, ty);
	 vec2 coord4 = vec2(tx, ty);
	 vec3 color1 = texture2D(vizpp_InputTex1, coord4).rgb;
	 vec3 color2 = texture2D(vizpp_InputTex2, coord4).rgb;
	 vec3 color3 = texture2D(vizpp_InputTex3, coord4).rgb;
	 vec3 color4 = texture2D(vizpp_InputTex4, coord4).rgb;
	 vec3 color5 = texture2D(vizpp_InputTex5, coord4).rgb;

	 float xx = mod(gl_FragCoord.x, 5.0);
	 float yy = mod(gl_FragCoord.y, 5.0);

	 float pre1 = floor(xx*0.5);
	 float pre2 = 3.0*mod(xx,2.0);
	 float pre3 = mod(pre1 + pre2 + yy, 5.0);
	 int rest = int(pre3);
	 if (rest == 3) {
	 color1.yz = vec2(0, 0);
	 color2.xz = vec2(0, 0);
	 color3.xy = vec2(0, 0);
	 color4.xyz = vec3(0, 0, 0);
	 color5.xyz = vec3(0, 0, 0);
	 }
	 if (rest == 0) {
	 color1.xyz = vec3(0, 0, 0);
	 color2.xyz = vec3(0, 0, 0);
	 color3.yz = vec2(0, 0);
	 color4.xz = vec2(0, 0);
	 color5.xy = vec2(0, 0);
	 }
	 if (rest == 2) {
	 color1.xz = vec2(0, 0);
	 color2.xy = vec2(0, 0);
	 color3.xyz = vec3(0, 0, 0);
	 color4.xyz = vec3(0, 0, 0);
	 color5.yz = vec2(0, 0);
	 }
	 if (rest == 4) {
	 color1.xyz = vec3(0, 0, 0);
	 color2.yz = vec2(0, 0);
	 color3.xz = vec2(0, 0);
	 color4.xy = vec2(0, 0);
	 color5.xyz = vec3(0, 0, 0);
	 }
	 if (rest == 1) 	{
	 color1.xy = vec2(0, 0);
	 color2.xyz = vec3(0, 0, 0);
	 color3.xyz = vec3(0, 0, 0);
	 color4.yz = vec2(0, 0);
	 color5.xz = vec2(0, 0);
	 }

	 gl_FragColor = vec4(color1 + color2 + color3 + color4 + color5,1.0);
*/
/*	int x,y, id1, id2, id3;
	 unsigned int subi, subj;
	 // Process the input images
	 for(unsigned int i = 0; i < output_height; ++i)
	 {
	 subi = i/5;
	 y = i%5;

	 if(i<=5)
	 std::cout << std::endl;
	 for(unsigned int j = 0; j < output_width; ++j)
	 {
	 subj = j/5;
	 x = j%5;
	 id1 = (start + y - (3*x)%number_input)%number_input;
	 id2 = id1-1;
	 if(id1 == 0) id2 = number_input-1;
	 id3 = id2-1;
	 if(id2 == 0) id3 = number_input-1;
	 if(i<5 && j<5)
	 {
	 std::cout << id1+1 << " " << id2+1 << " " << id3+1 << "-- ";

	 }
	 if(id1==0)
	 output.data[(i*output_width+j)*3] = input[id1].data[(subi*input[0].size().width+subj)*3];
	 if(id2==0)
	 output.data[(i*output_width+j)*3+1] = input[id2].data[(subi*input[0].size().width+subj)*3+1];
	 if(id3==0)
	 output.data[(i*output_width+j)*3+2] = input[id3].data[(subi*input[0].size().width+subj)*3+2];
	 }
	 }*/
	return true;
}
}

}
